perm filename PARALL.SAI[1,BGB] blob sn#001250 filedate 1972-10-22 generic text, type T, neo UTF8
00100	ENTRY PARALLAX;
00200	BEGIN	"PARALLAX"
00300		REQUIRE "ABBREV[SYS,BGB]" SOURCE_FILE;
00400		REQUIRE "TRIGER[SYS,BGB]" SOURCE_FILE;
00500		REQUIRE "COMMON[IA,BGB]" SOURCE_FILE;
00600	
00700	α FEATURE DATUM - 3D LOCUS;
00800		REAL ARRAY ∂F[1:5];	α X,Y,Z,R,dR;
00900		REAL ARRAY ITEMVAR F;
01000		INTEGER ITEMVAR TV;
     

00100	α SKEW RETURNS X,Y,Z OF THE MIDPOINT OF THE SHORTEST LINE SEGMENT;
00200	α BETWEEN THE SKEW LINES OF SIGHT & D THE LENGTH OF THAT SEGMENT;
00300	PROCEDURE SKEW (REAL ARRAY ITEMVAR CAM1,CAM2,SWN1,SWN2; REAL ARRAY XYZD);
00400	BEGIN	"SKEW"
00500		SAFE OWN REAL ARRAY C1,C2,R1,R2,S1,S2[1:3];
00600		REAL C11,C12,C21,C22,CC1,CC2,R12,X,Y,Z;
00700		DEFINE R11="10000",R22="10000",R21="R12";
00800		REAL SX,SY,K,K1,K2,R;
00900		INTEGER I;
01500		DEFINE DOT(X,Y)="X[1]*Y[1]+X[2]*Y[2]+X[3]*Y[3]";
01600		DEFINE THRICE="FOR I←1 STEP 1 UNTIL 3 DO";
01700	α GET THE LINE'O'SIGHT RAYS;
01800		SX	←	PDX/LDX;
01900		SY	←	PDY/LDY;
02000		X	←	SX*∂(SWN1)[1];
02100		Y	←	SY*∂(SWN1)[2];
02200		Z	←	-FOCAL;
02300		THRICE R1[I]←∂(CAM1)[1,I]*X + ∂(CAM1)[2,I]*Y + ∂(CAM1)[3,I]*Z;
02400		X	←	SX*∂(SWN2)[1];
02500		Y	←	SY*∂(SWN2)[2];
02600		THRICE R2[I]←∂(CAM2)[1,I]*X + ∂(CAM2)[2,I]*Y + ∂(CAM2)[3,I]*Z;
02700	α NORMALIZE R VECTORS TO A HEALTHY 100 FEET;
02800		R	←	100/SQRT(R1[1]↑2 + R1[2]↑2 + R1[3]↑2);
02900		THRICE	R1[I]←	R1[I]*R;
03000		R	←	100/SQRT(R2[1]↑2 + R2[2]↑2 + R2[3]↑2);
03100		THRICE	R2[I]←	R2[I]*R;
03200	α PICK UP THE CAMERA LOCUS VECTORS;
03300		ARRBLT(C1[1],∂(CAM1)[4,1],3);
03400		ARRBLT(C2[1],∂(CAM2)[4,1],3);
     

00100	α DIAGONOSTIC OUTPUT:
00200	BEGIN
00300		REAL AZM,ALT,RXY:
00400		INTEGER I,J:
00500		OUTSTR(↓&"SKEW DIAGONOSTIC."&↓):
00600		OUTSTR("CAM1"&↓&9):
00700		SETFORMAT(10,3):
00800		FOR I←1 STEP 1 UNTIL 4 DO
00900		FOR J←1 STEP 1 UNTIL 3 DO
01000		OUTSTR(CVF(∂(CAM1)[I,J])&(IF J=3 THEN ↓&9 ELSE 9)):
01100		OUTSTR(↓&"CAM2"&↓&9):
01200		SETFORMAT(10,3):
01300		FOR I←1 STEP 1 UNTIL 4 DO
01400		FOR J←1 STEP 1 UNTIL 3 DO
01500		OUTSTR(CVF(∂(CAM2)[I,J])&(IF J=3 THEN ↓&9 ELSE 9)):
01600		OUTSTR(↓&"SWN1"&↓):
01700		FOR I←1 STEP 1 UNTIL 4 DO
01800		OUTSTR(9&CVS(∂(SWN1)[I])):
01900		OUTSTR(↓):
02000		OUTSTR(↓&"SWN2"&↓):
02100		FOR I←1 STEP 1 UNTIL 4 DO
02200		OUTSTR(9&CVS(∂(SWN2)[I])):
02300		OUTSTR(↓):
02400		SETFORMAT(10,4):
02500		OUTSTR("	SX = "&CVG(SX)&↓):
02600		OUTSTR("	SY = "&CVG(SY)&↓):
02700		OUTSTR("R1 = "):THRICE OUTSTR(CVG(R1[I])&9):OUTSTR(↓):
02800		AZM	←	180*ATAN2(R1[2],R1[1])/π:
02900		RXY	←	SQRT(R1[2]↑2+R1[1]↑2):
03000		ALT	←	180*ATAN2(R1[3],RXY)/π:
03100		OUTSTR(9&"AZM = "&CVS(AZM)&9&"ALT = "&CVS(ALT)&↓):
03200		OUTSTR("R2 = "):THRICE OUTSTR(CVG(R2[I])&9):OUTSTR(↓):
03300		AZM	←	180*ATAN2(R2[2],R2[1])/π:
03400		RXY	←	SQRT(R2[2]↑2+R2[1]↑2):
03500		ALT	←	180*ATAN2(R2[3],RXY)/π:
03600		OUTSTR(9&"AZM = "&CVS(AZM)&9&"ALT = "&CVS(ALT)&↓):
03700		OUTSTR("C1 = "):THRICE OUTSTR(CVG(C1[I])&9):OUTSTR(↓):
03800		OUTSTR("C2 = "):THRICE OUTSTR(CVG(C2[I])&9):OUTSTR(↓):
03900		SETFORMAT(0,7):
04000		INCHRW:
04100	END;
     

00100	α EVALUATE DOT SOME DOT PRODUCTS;
00200		C11	←	DOT(C1,R1);
00300		C12	←	DOT(C1,R2);
00400		C21	←	DOT(C2,R1);
00500		C22	←	DOT(C2,R2);
00600		R12	←	DOT(R1,R2);
00700	α TWO MORE CONSTANTS;
00800		CC1	←	C21-C11;
00900		CC2	←	C22-C12;
01000	α USE KRAMER'S RULE TO SOLVE FOR THE PARAMETRIC VARIABLES;
01100		K	←	R12*R21 - R11*R22;
01200		K1	←	(CC2*R21 - CC1*R22)/K;
01300		K2	←	(R11*CC2 - R12*CC1)/K;
01400	α GET THE ENDPOINTS OF THE TRANSVERSAL SEGMENT;
01500		THRICE S1[I] ← C1[I] + K1*R1[I];
01600		THRICE S2[I] ← C2[I] + K2*R2[I];
01700	α RETURN THE TRANSVERSALS MIDPOINT AND LENGTH;
01800		THRICE XYZD[I] ← (S1[I]+S2[I])/2;
01900		XYZD[4]←SQRT((S1[1]-S2[1])↑2 +(S1[2]-S2[2])↑2 +(S1[3]-S2[3])↑2);
02000	α DIAGONOSTIC PRINT OUT:
02100		OUTSTR("S1 = "):THRICE OUTSTR(CVG(S1[I])&9):OUTSTR(↓):
02200		OUTSTR("S2 = "):THRICE OUTSTR(CVG(S2[I])&9):OUTSTR(↓):
02300		OUTSTR("XYZ= "):THRICE OUTSTR(CVG(XYZD[I])&9):OUTSTR(↓):
02400		OUTSTR("D  = "):OUTSTR(CVG(XYZD[4])&↓):
02500		INCHRW;
02600	END	"SKEW";
     

00100	α FEATURE LOCUS SOLVER BY PARALLAX;
00200	INTERNAL PROCEDURE PARALLAX;
00300	BEGIN	"PARALLAX"
00400		SET TMPSET;
00500		INTEGER I,J,K,M,N,VIEWS,WORST,FLG;
00600		REAL ARRAY XYZD[1:4];
00700		REAL X,Y,Z,R,RMAX,RSUM,RMEAN;
00800		ITEMVAR DUM1,DUM2;
00900	BEGIN	"GETF"
01000		STRING STR;	LABEL L;
01100	α STROBE THE USER FOR A FEATURE NAME;
01200	L:	OUTSTR(↓&9&"FEATURE = ");
01300		STR	←	INCHWL;
01400		F	←	CVSI(STR,FLG);
01500		IF FLG THEN GO L;
01600	END	"GETF";
01700	α COLLECT THE TVFILES THAT CONTAIN THIS FEATURE;
01800		TMPSET←PHI;
01900		∀ TV,DUM1,DUM2|TV⊗F≡DUM1∧LOCOR⊗TV≡DUM2 DO PUT TV IN TMPSET;
02000		VIEWS	←	LENGTH(TMPSET);
02100		IF VIEWS=0 THEN 
02200		BEGIN OUTSTR("NO VIEWS"&↓&"*");RETURN;END;
02300		OUTSTR(CVS(VIEWS)&" VIEWS."&↓);
02400		N	←	VIEWS;
02500		M	←	N*(N-1)%2;
     

00100	α ALLOCATE WORKING ARRAYS;
00200	BEGIN	"BLK2"
00300		REAL ARRAY D[1:N,1:N];	α DISTANCE BETWEEN RAYS;
00400		REAL ARRAY RAZALT[1:N,1:4];
00500		ITEMVAR ARRAY TVS,SWN,CAM[1:N];
00600		REAL ARRAY XYZ[1:M,1:3];
00700	α STASH EVERYTHING INTO THE WORKING ARRAYS;
00800		FOR I←1 STEP 1 UNTIL N DO
00900	BEGIN
01000		SET Q;
01100		IF LENGTH(TMPSET)=0 THEN OUTSTR("TMPSET FUCKED UP."&↓);
01200		TV	←	LOP(TMPSET);
01300		TVS[I]	←	TV;
01400		Q←TV⊗F; IF LENGTH(Q)=0 THEN OUTSTR("TV⊗F FUCKED UP."&↓);
01500		SWN[I]	←	LOP(Q);
01600		Q←LOCOR⊗TV; IF LENGTH(Q)=0 THEN OUTSTR("LOCOR⊗TV FUCKED UP."&↓)ELSE
01700		CAM[I]	←	LOP(Q);
01800	END;
     

00100	α SKEW EACH TV RAY TO THE FEATURE INTO ALL THE OTHERS;
00200		WHILE TRUE DO
00300	BEGIN	"BLK3"
00400		K	←	1;
00500		X←Y←Z	←	0;
00600		FOR I←1 STEP 1 UNTIL N-1 DO
00700		FOR J←I+1 STEP 1 UNTIL N DO
00800	BEGIN
00900		SKEW(CAM[I],CAM[J],SWN[I],SWN[J],XYZD);
01000		D[I,J]←D[J,I]←XYZD[4];
01100		ARRBLT(XYZ[K,1],XYZD[1],3);
01200		K	←	K+1;
01300		X←X+XYZD[1];	Y←Y+XYZD[2];	Z←Z+XYZD[3];
01400	END;
01500	α AVERAGE THE FEATURE LOCII;
01600		X←X/M;	Y←Y/M;	Z←Z/M;
01700	α COMPUTE THE MEAN AND MAX RADIUS OF FEATURE LOCII;
01800		RMAX ← RSUM ← 0;
01900		FOR K←1 STEP 1 UNTIL M DO
02000	BEGIN
02100		R ←	SQRT((XYZ[K,1]-X)↑2 +(XYZ[K,2]-Y)↑2 +(XYZ[K,3]-Z)↑2);
02200		RMAX ← R MAX RMAX;
02300		RSUM	←	RSUM + R;
02400	END;
02500		RMEAN	←	RSUM/M;
     

00100	α OUTPUT THE FEATURE LOCUS SOLUTION;
00200		OUTSTR(↓);
00300		OUTSTR("            ");
00400		OUTSTR("         X");
00500		OUTSTR("         Y");
00600		OUTSTR("         Z");
00700		OUTSTR("      RMAX");
00800		OUTSTR("     RMEAN");
00900		OUTSTR(↓);
01000		OUTSTR("FEATURE LOCUS	");
01100		SETFORMAT(10,3);
01200		OUTSTR(CVG(X));
01300		OUTSTR(CVG(Y));
01400		OUTSTR(CVG(Z));
01500		OUTSTR(CVG(RMAX));
01600		OUTSTR(CVG(RMEAN));
01700		OUTSTR(↓&↓);
01800	α COMPUTE THE AVERAGE TRANSVERAL LENGTHS FROM EACH CAMERA TO THE OTHERS;
01900		FOR I←1 STEP 1 UNTIL N DO
02000	BEGIN
02100		REAL SUM;
02200		SUM	←	0;
02300		FOR J←1 STEP 1 UNTIL N DO
02400		IF J≠I THEN	SUM←SUM+D[I,J];
02500		D[I,I]	←	SUM/(N-1);
02600	END;
     

00100	α OUTPUT THE RAZALT IN ORDER ON MEAN D BEST TO WORST;
00200		OUTSTR("TVFILE	 DMEAN    RANGE     AZIMUTH   ALTITUDE"&↓);
00300		WHILE TRUE DO
00400	BEGIN
00500		REAL BEST,DX,DY,DZ,RANGE,AZM,ALT,RXY;
00600		REAL ARRAY ITEMVAR C;
00700		WORST	←	J;
00800		J ← 0;	BEST ← 999999;
00900		FOR I←1 STEP 1 UNTIL N DO IF D[I,I]<BEST THEN
01000		BEGIN BEST←D[I,I];J←I;END;
01100		IF J=0 THEN DONE;
01200		D[J,J]	← 1000000;
01300		OUTSTR(CVIS(TVS[J],FLG)&9);
01400		OUTSTR(CVG(BEST));
01500		C	←	CAM[J];
01600		DX	←	X - ∂(C)[4,1];
01700		DY	←	Y - ∂(C)[4,2];
01800		DZ	←	Z - ∂(C)[4,3];
01900		RANGE	←	SQRT(DX↑2+DY↑2+DZ↑2);
02000		RXY	←	SQRT(DX↑2+DY↑2);
02100		AZM	←	180*ATAN2(DY,DX)/π;
02200		ALT	←	180*ATAN2(DZ,RXY)/π;
02300		OUTSTR(CVG(RANGE));
02400		OUTSTR(CVG(AZM));
02500		OUTSTR(CVG(ALT));
02600		OUTSTR(↓);
02700	END;
     

00100	α SWITCH OPTIONS;
00200	BEGIN	"SWITCH"
00300		INTEGER CHR;
00400		LABEL L;
00500	L:	OUTSTR(↓&"+,-,Y,N,R SWITCH ?");
00600		CHR	←	INCHRW;
00700		IF CHR="Y" THEN
00800	BEGIN
00900		∂(F)[1]	←	X;
01000		∂(F)[2]	←	Y;
01100		∂(F)[3]	←	Z;
01200		∂(F)[5]	←	RMEAN;
01300		OUTSTR(↓&"*");
01400		RETURN;
01500	END	ELSE
01600		IF CHR="N" THEN BEGIN OUTSTR(↓&"*");RETURN;END ELSE
01700		IF CHR="+" ∧ N≠VIEWS THEN N←N+1 ELSE
01800		IF CHR="-" ∧ N≠2 THEN
01900	BEGIN
02000		TVS[WORST]↔TVS[N];
02100		SWN[WORST]↔SWN[N];
02200		CAM[WORST]↔CAM[N];
02300		N ← N-1;
02400	END	ELSE	GO L;
02500		M	←	N*(N-1)%2;
02600	END	"SWITCH";
02700	END	"BLK3";
02800	END	"BLK2";
02900	END	"PARALLAX";
03000	
03100	END	"PARALLAX";